﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace InverseSqrtBenchmark {
    public struct Result {
        public double error;
        public TimeSpan elapsed;
    }
    class Program {
        static void Main(string[] args) {
            float MaxFloatSqrt = (float)Math.Sqrt(float.MaxValue);
            const int Loops = 1000 * 1000 * 100;
            const int Trials = 1;
            Stopwatch watch = new Stopwatch();
            Random random = new Random();

            float[] frands = new float[Trials];
            double[] drands = new double[Trials];
            for (uint i = 0; i < Trials; i++) {
                frands[i] = MaxFloatSqrt * (float)random.NextDouble();
                drands[i] = frands[i];
            }

            for (int i = 0; i < Trials; i++) {
                float sq = frands[i];
                double dsq = drands[i];
                float ftmp = 0;
                double dtmp = 0;
                Result[] results = new Result[4];
                uint index = 0;

                results[index].error = Math.Abs(findInverseSqrt(dsq) - 1.0 / Math.Sqrt(dsq)) * Math.Sqrt(dsq);
                watch.Reset();
                watch.Start();
                for (uint j = 0; j < Loops; j++)
                    dtmp += findInverseSqrt(dsq);
                watch.Stop();
                results[index].elapsed = watch.Elapsed;

                results[++index].error = Math.Abs(InvSqrt(sq) - 1.0 / Math.Sqrt(dsq)) * Math.Sqrt(dsq);
                watch.Reset();
                watch.Start();
                for (uint j = 0; j < Loops; j++)
                    ftmp += InvSqrt(sq);
                watch.Stop();
                results[index].elapsed = watch.Elapsed;

                results[++index].error = Math.Abs(InvSqrt(dsq) - 1.0 / Math.Sqrt(dsq)) * Math.Sqrt(dsq);
                watch.Reset();
                watch.Start();
                for (uint j = 0; j < Loops; j++)
                    dtmp += InvSqrt(dsq);
                watch.Stop();
                results[index].elapsed = watch.Elapsed;

                results[++index].error = 0;
                watch.Reset();
                watch.Start();
                for (uint j = 0; j < Loops; j++)
                    dtmp += 1.0 / Math.Sqrt(dsq);
                watch.Stop();
                results[index].elapsed = watch.Elapsed;

               
             Console.WriteLine(String.Format("{0:e},{1},{2},{3},{4},{5:e},{6:e},{7:e},{8:e}", 
                    sq, 
                    results[0].elapsed, results[1].elapsed, results[2].elapsed, results[3].elapsed, 
                    results[0].error, results[1].error, results[2].error, results[3].error));
            }
        }
        public static double findInverseSqrt(double a) {
            const double eps = 1.0e-7;
            double x = 2.0 / (a + 1);
            double ahalf = a * 0.5f;
            
            double residue;
            do {
                residue = x * (0.5 - ahalf * x * x);
                x += residue;
            } while (Math.Abs(residue) > eps);

            return x;
        }
        public static float InvSqrt(float x) {
            float xhalf = 0.5f * x;
            unsafe {
                UInt32 i = *(UInt32*)&x; // store floating-point bits in integer
                i = 0x5f3759d5 - (i >> 1); // initial guess for Newton's method
                x = *(float*)&i; // convert new bits into float
            }
            x = x * (1.5f - xhalf * x * x); // One round of Newton's method
            return x;
        }
        public static double InvSqrt(double x) {
            double xhalf = 0.5 * x;
            unsafe {
                UInt64 i = *(UInt64*)&x; // store floating-point bits in integer
                i = 0x5fe6ec85e7de30da - (i >> 1); // initial guess for Newton's method
                x = *(double*)&i; // convert new bits into float
            }
            x = x * (1.5 - xhalf * x * x); // One round of Newton's method
            return x;
        }
    }
}
